import rospy
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

import cv2
from hikvision_msgs.srv import PTZSetService, PTZGetService


class SensorManager():
    """
    This class is used to control camera and capture images using the kinect sensor
    """
    def __init__(self, topic="/image_raw"):
        # rospy.init_node('sensor_manager')

        self.bridge = CvBridge()
        # store the last image captured
        self.last_tohand_image = [None, None]
        # Define the image topic
        tohand_image_topic = topic

        rospy.Subscriber(tohand_image_topic, Image, self.__tohand_image_callback)

        rospy.loginfo('Waiting for PTZSetService...')
        rospy.wait_for_service('/hik_cam_node/hik_camera/set_ptz_value', timeout=10)
        self.__set_ptz = rospy.ServiceProxy('/hik_cam_node/hik_camera/set_ptz_value', PTZSetService)

        rospy.loginfo('Waiting for PTZGetService...')
        rospy.wait_for_service('/hik_cam_node/hik_camera/get_ptz_value', timeout=10)
        self.__get_ptz = rospy.ServiceProxy('/hik_cam_node/hik_camera/get_ptz_value', PTZGetService)

        rospy.loginfo('SensorManager loaded.')


    def __tohand_image_callback(self, msg):
        """
        Saves the last published tohand camera image to last_tohand_image

        :param msg: The ROS message
        """
        # timestamp = (msg.header.stamp.secs, msg.header.stamp.nsecs)
        try:
            cv2_img = self.bridge.imgmsg_to_cv2(msg, 'rgb8')
        except CvBridgeError as e:
            print(e)
        else:
            # self.last_tohand_image[0] = (cv2_img, timestamp)
            self.last_tohand_image[1] = self.last_tohand_image[0]
            self.last_tohand_image[0] = cv2_img

    def get_image(self):
        """
        Captures an image with a time stamp greater than the current time. This helps us overcome
        ROS synchronization issues and ensures that we don't get images from the past
        """

        last_image = self.last_tohand_image[1]

        # # current time
        # now = rospy.get_rostime() - rospy.Time(secs=0)

        # # ensure that the time stamp of the image is greater than the current time by at least 0.5s
        # while (now + rospy.Duration(0, 500000000)) > rospy.Duration(last_image[0][1][0],
        #                                                             last_image[0][1][1]):
        #     rospy.sleep(0.1)
        return last_image

    def save_image(self, image, filename='./output.jpg'):
        return cv2.imwrite(filename, image)
    
    # Get PTZ
    def get_pan(self):
        return self.__get_ptz(1).pan
    def get_tilt(self):
        return self.__get_ptz(1).tilt
    def get_zoom(self):
        return self.__get_ptz(1).zoom

    # Set PTZ
    def set_pan(self, pan_value):
        ptz = self.__get_ptz(1)
        ptz.pan = pan_value
        self.__set_ptz(ptz)
    def set_tilt(self, tile_value):
        ptz = self.__get_ptz(1)
        ptz.tilt = tile_value
        self.__set_ptz(ptz)
    def set_zoom(self, zoom_value):
        ptz = self.__get_ptz(1)
        ptz.zoom = zoom_value
        self.__set_ptz(ptz)


if __name__ == "__main__":
    rospy.init_node('SENSOR_MANAGER', anonymous=True)
    
    sm = SensorManager()

    print(sm.get_pan())
    print(sm.get_tilt())
    print(sm.get_zoom())

    # activate the gripper
    img = sm.get_image()

    if sm.save_image(img, '../test_image.jpg'):
        print('Image saved.')
